#include "lidar_point_cloud_correct.h"
#define cells_i_nums  600
float ground_threshold_value = 0.15;
float cell_size = 0.2;
float dis_limit = 10;
float angle_limit = 10;

gridObjPoint All16LiDARCells[cells_i_nums][cells_i_nums];

LidarPointCloudCorrect::LidarPointCloudCorrect(ros::NodeHandle *nodehandle):nh_(*nodehandle)
{
  dm_ = new DataManagement(&nh_);
}

LidarPointCloudCorrect::~LidarPointCloudCorrect()
{
  delete dm_;
}

bool LidarPointCloudCorrect::init()
{
    if(!initConfig())
    {
        PrintE("init config failed");
        return false;
    }
    dm_->init();
    initialization = true;
    PrintI("LidarPointCloudCorrect init ok!!");
    return true;
}

bool LidarPointCloudCorrect::initConfig()
{
    std::fstream fp;
    nlohmann::json json_object;
    fp.open("../config/calibration_parameters.json", std::ios::in);
    if (!fp.is_open())
        return false;

     fp >> json_object;
     for(uint8_t i=0;i<json_object["LidarInfo"].size();i++)
     {
        sensor_correct_param_.type = json_object["LidarInfo"][i]["Type"].get<int>();           
        sensor_correct_param_.serial_num = json_object["LidarInfo"][i]["SerialNum"].get<std::string>();
        sensor_correct_param_.desc = json_object["LidarInfo"][i]["Comment"].get<std::string>();
        sensor_correct_param_.param.pose_position_x = json_object["LidarInfo"][i]["X"].get<double>();
        sensor_correct_param_.param.pose_position_y = json_object["LidarInfo"][i]["Y"].get<double>();
        sensor_correct_param_.param.pose_position_z = json_object["LidarInfo"][i]["Z"].get<double>();
        sensor_correct_param_.param.euler_angle_yaw = json_object["LidarInfo"][i]["YAW"].get<double>();
        sensor_correct_param_.param.euler_angle_pitch = json_object["LidarInfo"][i]["PITCH"].get<double>();    
        sensor_correct_param_.param.euler_angle_roll = json_object["LidarInfo"][i]["ROLL"].get<double>();
        Eigen::Quaterniond quaternion;
        quaternion = Eigen::AngleAxisd(sensor_correct_param_.param.euler_angle_yaw, Eigen::Vector3d::UnitZ())
                    * Eigen::AngleAxisd(sensor_correct_param_.param.euler_angle_pitch, Eigen::Vector3d::UnitX())
                    * Eigen::AngleAxisd(sensor_correct_param_.param.euler_angle_roll, Eigen::Vector3d::UnitY());

        Eigen::Vector3d translation(sensor_correct_param_.param.pose_position_x, sensor_correct_param_.param.pose_position_y, sensor_correct_param_.param.pose_position_z);
        sensor_correct_param_.param.transformation.setIdentity();
        sensor_correct_param_.param.transformation.translate(translation);
        sensor_correct_param_.param.transformation.rotate(quaternion);
    }
    return true;
}

void LidarPointCloudCorrect::process()
{
    while(nh_.ok())
    {
       pcl::PointCloud<pcl::PointXYZI> point_cloud2 =  DataProviderRawPointCloud::Instance()->lspoint_cloud();
       if (point_cloud2.points.size() > 0)
       {
           pointCloudCalibration(sensor_correct_param_, point_cloud2);
           pcl::PointCloud<pcl::PointXYZI> pre_point_cloud = pointCloudPreProcess(point_cloud2);
           if(pointCloudGridFilter(pre_point_cloud))
           {
                groundFiltering();
                bfsGridSearch();
           }
       }
       usleep(50000);
    }
}

void LidarPointCloudCorrect::bfsGridSearch()
{
        vector<int> ilist2;
        vector<int> jlist2;
        int seq_num = 0;
        int gridSearchFlag[cells_i_nums][cells_i_nums] = {0};
        All16LiDAROb.lidar_objs.clear();
        for (int i = 0; i < cells_i_nums; i++)
            for(int j=0;j<cells_i_nums;j++)
            {                                                                                                        
                int tempi=i;
                int tempj=j;
                if(All16LiDARCells[tempi][tempj].upper_ob==1&&gridSearchFlag[tempi][tempj]==0)
                {
                    int k=1;
                    gridSearchFlag[tempi][tempj]=1;
                    ilist2.push_back(tempi);
                    jlist2.push_back(tempj);
                    for(;;)
                    {
                        if(tempi+1<cells_i_nums&&All16LiDARCells[tempi+1][tempj].upper_ob==1)//1right
                        {
                            if(gridSearchFlag[tempi+1][tempj]==0)
                            {
                                ilist2.push_back(tempi+1);
                                jlist2.push_back(tempj);
                                gridSearchFlag[tempi+1][tempj]=1;
                            }
                        }
                        if(tempi+1<cells_i_nums&&tempj+1<cells_i_nums&&All16LiDARCells[tempi+1][tempj+1].upper_ob==1)//2 right Up
                        {
                            if(gridSearchFlag[tempi+1][tempj+1]==0)
                            {
                                ilist2.push_back(tempi+1);
                                jlist2.push_back(tempj+1);
                                gridSearchFlag[tempi+1][tempj+1]=1;
                            }
                        }
                        if(tempi<cells_i_nums&&tempj+1<cells_i_nums&&All16LiDARCells[tempi][tempj+1].upper_ob==1)//3 Up
                        {
                            if(gridSearchFlag[tempi][tempj+1]==0)
                            {
                                ilist2.push_back(tempi);
                                jlist2.push_back(tempj+1);
                                gridSearchFlag[tempi][tempj+1]=1;
                            }
                        }
                        if(tempi-1>=0&&tempj+1<cells_i_nums&&All16LiDARCells[tempi-1][tempj+1].upper_ob==1)//4 Right Up
                        {
                            if(gridSearchFlag[tempi][tempj+1]==0)
                            {
                                ilist2.push_back(tempi-1);
                                                                jlist2.push_back(tempj+1);
                                gridSearchFlag[tempi-1][tempj+1]=1;
                            }
                        }

                        if(tempi-1>=0&&All16LiDARCells[tempi-1][tempj].upper_ob==1)//5 Right
                        {
                            if(gridSearchFlag[tempi-1][tempj]==0)
                            {
                                ilist2.push_back(tempi-1);
                                jlist2.push_back(tempj);
                                gridSearchFlag[tempi-1][tempj]=1;
                            }
                        }

                        if(tempi-1>=0&&tempj-1>=0&&All16LiDARCells[tempi-1][tempj-1].upper_ob==1)//6 Right down
                        {
                            if(gridSearchFlag[tempi-1][tempj-1]==0)
                            {
                                ilist2.push_back(tempi-1);
                                jlist2.push_back(tempj-1);
                                gridSearchFlag[tempi-1][tempj-1]=1;
                            }
                        }

                        if(tempj-1>=0&&All16LiDARCells[tempi][tempj-1].upper_ob==1)//7 down
                        {
                            if(gridSearchFlag[tempi][tempj-1]==0)
                            {
                                ilist2.push_back(tempi);
                                jlist2.push_back(tempj-1);
                                gridSearchFlag[tempi][tempj-1]=1;
                            }
                        }

                        if(tempi+1<cells_i_nums&&tempj-1>=0&&All16LiDARCells[tempi+1][tempj-1].upper_ob==1)//8 right down
                        {
                            if(gridSearchFlag[tempi+1][tempj-1]==0)
                            {
                                ilist2.push_back(tempi+1);
                                jlist2.push_back(tempj-1);
                                gridSearchFlag[tempi+1][tempj-1]=1;
                            }
                        }

                                                
                        auto max_ilist2Position = max_element(ilist2.begin(), ilist2.end());
                        auto min_ilist2Position = min_element(ilist2.begin(), ilist2.end());
                        auto max_jlist2Position = max_element(jlist2.begin(), jlist2.end());
                        auto min_jlist2Position = min_element(jlist2.begin(), jlist2.end());
                        if((ilist2.size()==k)||(*max_jlist2Position - *min_jlist2Position > 20)||(*max_ilist2Position - *min_ilist2Position > 20))
                        {
                            lidar_detect::lidar_3d TempLiDAROb;
                            int iMinj=800;
                            int iMaxj=-800;
                            int jMini=800;
                            int jMaxi=-800;
                            int iMin=800;
                            int iMax=-800;
                            int jMin=800;
                            int jMax=-800;
                            float Zmin=10000;
                            float Zmax=-10000; 
                            int ob_point_cloud_nums=0;
                            for(int u=0;u<ilist2.size();u++)
                            {
                                if(ilist2[u]<=iMin)
                                {
                                    iMin=ilist2[u];
                                    iMinj=jlist2[u];
                                }                                                                                    
                                if(ilist2[u]>=iMax)
                                {
                                    iMax=ilist2[u];
                                    iMaxj=jlist2[u];

                                }
                                if(jlist2[u]<=jMin)
                                {
                                    jMin=jlist2[u];
                                    jMini=ilist2[u];

                                }
                                if(jlist2[u]>=jMax)
                                {
                                    jMax=jlist2[u];
                                    jMaxi=ilist2[u];
                                }
                                if(All16LiDARCells[ilist2[u]][jlist2[u]].upper_zMin<Zmin)
                                    Zmin=All16LiDARCells[ilist2[u]][jlist2[u]].upper_zMin;
                                if(All16LiDARCells[ilist2[u]][jlist2[u]].upper_zMax>Zmax)
                                    Zmax=All16LiDARCells[ilist2[u]][jlist2[u]].upper_zMax;

                                ob_point_cloud_nums=ob_point_cloud_nums+All16LiDARCells[ilist2[u]][jlist2[u]].UpperCloudPoints.size();
                                for(int points_count=0;points_count<All16LiDARCells[ilist2[u]][jlist2[u]].UpperCloudPoints.size();points_count++)
                                {
                                    TempLiDAROb.CloudPoints.push_back(All16LiDARCells[ilist2[u]][jlist2[u]].UpperCloudPoints[points_count]);
                                }
                            }
                            //找到最外层轮廓的所有栅格
                            vector<int> ilistContour;
                            vector<int> jlistContour;
                            lidar_detect::keypoint KeyPoint;
                            for(int v=0;v<ilist2.size();v++)
                            {
                                if(ilist2[v]+1<cells_i_nums
                                        &&All16LiDARCells[ilist2[v]+1][jlist2[v]].upper_ob!=1)//1right
                                {
                                    ilistContour.push_back(ilist2[v]);
                                    jlistContour.push_back(jlist2[v]);
                                    continue;
                                }
                                if(ilist2[v]+1<cells_i_nums
                                        &&jlist2[v]+1<cells_i_nums
                                        &&All16LiDARCells[ilist2[v]+1][jlist2[v]+1].upper_ob!=1)//2 right Up
                                {                                                                                    
                                    ilistContour.push_back(ilist2[v]);
                                    jlistContour.push_back(jlist2[v]);
                                    continue;
                                }
                                if(ilist2[v]<cells_i_nums
                                        &&jlist2[v]+1<cells_i_nums
                                        &&All16LiDARCells[ilist2[v]][jlist2[v]+1].upper_ob!=1)//3 Up
                                {
                                    ilistContour.push_back(ilist2[v]);
                                    jlistContour.push_back(jlist2[v]);
                                    continue;
                                }
                                if(ilist2[v]-1>=0
                                        &&jlist2[v]+1<cells_i_nums
                                        &&All16LiDARCells[ilist2[v]-1][jlist2[v]+1].upper_ob!=1)//4 Right Up
                                {
                                    ilistContour.push_back(ilist2[v]);
                                    jlistContour.push_back(jlist2[v]);
                                    continue;
                                }

                                if(ilist2[v]-1>=0
                                        &&All16LiDARCells[ilist2[v]-1][jlist2[v]].upper_ob!=1)//5 Right
                                {
                                    ilistContour.push_back(ilist2[v]);
                                    jlistContour.push_back(jlist2[v]);
                                    continue;
                                }

                                if(ilist2[v]-1>=0
                                        &&jlist2[v]-1>=0
                                        &&All16LiDARCells[ilist2[v]-1][jlist2[v]-1].upper_ob!=1)//6 Right down
                                {
                                    ilistContour.push_back(ilist2[v]);
                                    jlistContour.push_back(jlist2[v]);     
                                    continue;
                                }

                                if(jlist2[v]-1>=0
                                        &&All16LiDARCells[ilist2[v]][jlist2[v]-1].upper_ob!=1)//7 down
                                {
                                    ilistContour.push_back(ilist2[v]);
                                    jlistContour.push_back(jlist2[v]);
                                    continue;
                                }
                                if(ilist2[v]+1<cells_i_nums
                                        &&jlist2[v]-1>=0
                                        &&All16LiDARCells[ilist2[v]+1][jlist2[v]-1].upper_ob!=1)//8 right down
                                {
                                    ilistContour.push_back(ilist2[v]);
                                    jlistContour.push_back(jlist2[v]);
                                    continue;
                                }
                            }

                            for(int v=0;v<ilistContour.size();v++)
                            {
                                KeyPoint.x=All16LiDARCells[ilistContour[v]][jlistContour[v]].upper_xMin;
                                KeyPoint.y=All16LiDARCells[ilistContour[v]][jlistContour[v]].upper_yMax;
                                TempLiDAROb.ContourPointsVector.push_back(KeyPoint);
                            }
                            TempLiDAROb.id = seq_num++;
                            TempLiDAROb.left_limit_position.x = All16LiDARCells[iMin][iMinj].upper_xMin;
                            TempLiDAROb.left_limit_position.y=All16LiDARCells[iMin][iMinj].upper_yMin;
                            TempLiDAROb.right_limit_position.x=All16LiDARCells[iMax][iMaxj].upper_xMax;
                            TempLiDAROb.right_limit_position.y=All16LiDARCells[iMax][iMaxj].upper_yMax;
                            TempLiDAROb.front_limit_position.x=All16LiDARCells[jMaxi][jMax].upper_xMax;
                            TempLiDAROb.front_limit_position.y=All16LiDARCells[jMaxi][jMax].upper_yMax;
                            TempLiDAROb.rear_limit_position.x=All16LiDARCells[jMini][jMin].upper_xMax;
                            TempLiDAROb.rear_limit_position.y=All16LiDARCells[jMini][jMin].upper_yMax;
                            TempLiDAROb.ObLength=abs(All16LiDARCells[jMaxi][jMax].upper_yMax-All16LiDARCells[jMini][jMin].upper_yMin);                         
                            TempLiDAROb.ObWidth=abs(All16LiDARCells[iMax][iMaxj].upper_xMax-All16LiDARCells[iMin][iMinj].upper_xMin);

                            TempLiDAROb.ObHeight = Zmax - Zmin;
                            TempLiDAROb.CentralZ = (Zmax - Zmin)/2;                                                  
                            TempLiDAROb.CentralY=(TempLiDAROb.left_limit_position.y+TempLiDAROb.right_limit_position.y+TempLiDAROb.front_limit_position.y+TempLiDAROb.rear_limit_position.y)/4;
                            TempLiDAROb.CentralX=(TempLiDAROb.left_limit_position.x+TempLiDAROb.right_limit_position.x+TempLiDAROb.front_limit_position.x+TempLiDAROb.rear_limit_position.x)/4;
                            TempLiDAROb.distance_1 = std::sqrt(TempLiDAROb.CentralY*TempLiDAROb.CentralY+TempLiDAROb.CentralX*TempLiDAROb.CentralX);
                            TempLiDAROb.StaticOb = 0;

                            if(ob_point_cloud_nums>0)
                                All16LiDAROb.lidar_objs.push_back(TempLiDAROb);
                            ilist2.clear();
                            jlist2.clear();
                            break;
                        }
                        else
                        {
                            tempi=ilist2[k];
                            tempj=jlist2[k];
                            k++;
                        }
                    }
                }
            }

        dm_->lidar_objects_publisher_.publish(All16LiDAROb);
}

bool LidarPointCloudCorrect::pointCloudGridFilter(pcl::PointCloud<pcl::PointXYZI> point_cloud)
{
    for(int i=0;i<cells_i_nums;i++)
        for(int j=0;j<cells_i_nums;j++)
        {    
            All16LiDARCells[i][j].lowwer_ob=0;                                                                    
            All16LiDARCells[i][j].upper_ob=0;
            All16LiDARCells[i][j].UpperCloudPoints.clear();
            All16LiDARCells[i][j].LowwerCloudPoints.clear();
            All16LiDARCells[i][j].upper_xMax=-10000;
            All16LiDARCells[i][j].upper_yMax=-10000;
            All16LiDARCells[i][j].upper_zMax=-10000;
            All16LiDARCells[i][j].upper_xMin=10000;
            All16LiDARCells[i][j].upper_yMin=10000;
            All16LiDARCells[i][j].upper_zMin=10000;
            All16LiDARCells[i][j].lowwer_xMax=-10000;
            All16LiDARCells[i][j].lowwer_yMax=-10000;
            All16LiDARCells[i][j].lowwer_zMax=-10000;
            All16LiDARCells[i][j].lowwer_xMin=10000;
            All16LiDARCells[i][j].lowwer_yMin=10000;
            All16LiDARCells[i][j].lowwer_zMin=10000;
        }

    for (int i = 0; i < point_cloud.points.size();i++)
    {
        int in = 0;
        int jn = 0;
        in = point_cloud.points[i].x / cell_size + cells_i_nums / 2;
        jn = point_cloud.points[i].y / cell_size + cells_i_nums / 2;
        if(in>=0&&in<cells_i_nums&&jn>=0&&jn<cells_i_nums)
        {
            lidar_detect::raw_data_point point;
            point.x = point_cloud.points[i].x;
            point.y = point_cloud.points[i].y;
            point.z = point_cloud.points[i].z;
            point.intensity = point_cloud.points[i].intensity;
            if (point.z > ground_threshold_value)
            {
                All16LiDARCells[in][jn].UpperCloudPoints.push_back(point);
                if(point.x>All16LiDARCells[in][jn].upper_xMax)
                    All16LiDARCells[in][jn].upper_xMax=point.x;
                if(point.y>All16LiDARCells[in][jn].upper_yMax)
                    All16LiDARCells[in][jn].upper_yMax=point.y;
                if(point.y<All16LiDARCells[in][jn].upper_yMin)
                    All16LiDARCells[in][jn].upper_yMin=point.y;
                if(point.x<All16LiDARCells[in][jn].upper_xMin)
                    All16LiDARCells[in][jn].upper_xMin=point.x;
                if(point.z<All16LiDARCells[in][jn].upper_zMin)
                    All16LiDARCells[in][jn].upper_zMin=point.z;
                if(point.z>All16LiDARCells[in][jn].upper_zMax)
                    All16LiDARCells[in][jn].upper_zMax=point.z;
                if(All16LiDARCells[in][jn].UpperCloudPoints.size()>=3) 
                    All16LiDARCells[in][jn].upper_ob=1;
            }else{
                All16LiDARCells[in][jn].LowwerCloudPoints.push_back(point);
                if(point.x>All16LiDARCells[in][jn].lowwer_xMax)
                    All16LiDARCells[in][jn].lowwer_xMax=point.x;
                if(point.y>All16LiDARCells[in][jn].lowwer_yMax)
                    All16LiDARCells[in][jn].lowwer_yMax=point.y;
                if(point.y<All16LiDARCells[in][jn].lowwer_yMin)
                    All16LiDARCells[in][jn].lowwer_yMin=point.y;
                if(point.x<All16LiDARCells[in][jn].lowwer_xMin)
                    All16LiDARCells[in][jn].lowwer_xMin=point.x;
                if(point.z<All16LiDARCells[in][jn].lowwer_zMin)
                    All16LiDARCells[in][jn].lowwer_zMin=point.z;
                if(point.z>All16LiDARCells[in][jn].lowwer_zMax)
                    All16LiDARCells[in][jn].lowwer_zMax=point.z;
                if(All16LiDARCells[in][jn].LowwerCloudPoints.size()>=3)
                    All16LiDARCells[in][jn].lowwer_ob=1;
            }
        }
    }

    return true;
}

void LidarPointCloudCorrect::groundFiltering()
{
    vector<int> i_ground_cell_vector;
    vector<int> j_ground_cell_vector;
    for(int i=cells_i_nums/9.0*3;i<cells_i_nums/9.0*6;i++)
        for(int j=cells_i_nums/9.0*3;j<cells_i_nums/9.0*6;j++)
        {
            if(All16LiDARCells[i][j].lowwer_ob==1&&All16LiDARCells[i][j].lowwer_zMax<cell_size) 
            {
                All16LiDARCells[i][j].lowwer_ob=4;
                i_ground_cell_vector.push_back(i);
                j_ground_cell_vector.push_back(j);
            }
        }

    while(i_ground_cell_vector.size()!=0)
        liDARGroundCluster(dis_limit,angle_limit,cell_size,i_ground_cell_vector,j_ground_cell_vector);
    for(int i=0;i<cells_i_nums;i++)
        for(int j=0;j<cells_i_nums;j++)
        {
            if(All16LiDARCells[i][j].lowwer_ob==1)
            {
                All16LiDARCells[i][j].upper_ob=1;
                if(All16LiDARCells[i][j].upper_zMin>All16LiDARCells[i][j].lowwer_zMin)
                    All16LiDARCells[i][j].upper_zMin=All16LiDARCells[i][j].lowwer_zMin;
                if(All16LiDARCells[i][j].upper_xMin>All16LiDARCells[i][j].lowwer_xMin)
                    All16LiDARCells[i][j].upper_xMin=All16LiDARCells[i][j].lowwer_xMin;
                if(All16LiDARCells[i][j].upper_yMin>All16LiDARCells[i][j].lowwer_yMin)                           
                    All16LiDARCells[i][j].upper_yMin=All16LiDARCells[i][j].lowwer_yMin;
                if(All16LiDARCells[i][j].upper_yMax<All16LiDARCells[i][j].lowwer_yMax)
                    All16LiDARCells[i][j].upper_yMax=All16LiDARCells[i][j].lowwer_yMax;
                if(All16LiDARCells[i][j].upper_xMax<All16LiDARCells[i][j].lowwer_xMax)
                    All16LiDARCells[i][j].upper_xMax=All16LiDARCells[i][j].lowwer_xMax;
                if(All16LiDARCells[i][j].upper_zMax<All16LiDARCells[i][j].lowwer_zMax)
                    All16LiDARCells[i][j].upper_zMax=All16LiDARCells[i][j].lowwer_zMax;
                for(int points_count=0;points_count<All16LiDARCells[i][j].LowwerCloudPoints.size();points_count++)
                {
                    All16LiDARCells[i][j].UpperCloudPoints.push_back(All16LiDARCells[i][j].LowwerCloudPoints[points_count]);
                }
            }
        }
}

void LidarPointCloudCorrect::liDARGroundCluster(float dis_limit, float limit_angle, float cell_size, vector<int> &i_ground_cell_vector, vector<int> &j_ground_cell_vector)
{
    vector<int> new_i_ground_cell_vector;
    vector<int> new_j_ground_cell_vector;
    for(int i=0;i<i_ground_cell_vector.size();i++)
    {    
        float angle_limit_tan=tan(limit_angle/180.0*3.1415);
        int temp_i=i_ground_cell_vector[i];
        int temp_j=j_ground_cell_vector[i];
        //向正右方搜索
        oneDirectionCellJudge(dis_limit,angle_limit_tan,cell_size,1,0,temp_i,temp_j,new_i_ground_cell_vector,new_j_ground_cell_vector);
        //向右上方搜索
        oneDirectionCellJudge(dis_limit,angle_limit_tan,cell_size,1,1,temp_i,temp_j,new_i_ground_cell_vector,new_j_ground_cell_vector);
        //向正上方搜索
        oneDirectionCellJudge(dis_limit,angle_limit_tan,cell_size,0,1,temp_i,temp_j,new_i_ground_cell_vector,new_j_ground_cell_vector);
        //向左上方搜索
        oneDirectionCellJudge(dis_limit,angle_limit_tan,cell_size,-1,1,temp_i,temp_j,new_i_ground_cell_vector,new_j_ground_cell_vector);
        //向正左方搜索
        oneDirectionCellJudge(dis_limit,angle_limit_tan,cell_size,-1,0,temp_i,temp_j,new_i_ground_cell_vector,new_j_ground_cell_vector);
        //向左下方搜索
        oneDirectionCellJudge(dis_limit,angle_limit_tan,cell_size,-1,-1,temp_i,temp_j,new_i_ground_cell_vector,new_j_ground_cell_vector);
        //向正下方搜索
        oneDirectionCellJudge(dis_limit,angle_limit_tan,cell_size,1,-1,temp_i,temp_j,new_i_ground_cell_vector,new_j_ground_cell_vector);
    }
    i_ground_cell_vector.clear();
    j_ground_cell_vector.clear();
    i_ground_cell_vector=new_i_ground_cell_vector;
    j_ground_cell_vector=new_j_ground_cell_vector;

}

void  LidarPointCloudCorrect::oneDirectionCellJudge(float dis_limit,float angle_limit_tan,float cell_height_threshold,int m_i,int m_j,int temp_i,int temp_j,
                                           vector<int>& i_ground_cell_vector,
                                           vector<int>& j_ground_cell_vector)
{
    int m=1;
    while(true)
    {
                                                                                                                     
        if(temp_i+m_i*m<cells_i_nums
                &&temp_j+m_j*m<cells_i_nums
                &&temp_i+m_i*m>=0
                &&temp_j+m_j*m>=0
                &&(All16LiDARCells[temp_i+m_i*m][temp_j+m_j*m].lowwer_ob==1
                   ||All16LiDARCells[temp_i+m_i*m][temp_j+m_j*m].lowwer_ob==0))
        {
            if(All16LiDARCells[temp_i+m_i*m][temp_j+m_j*m].lowwer_ob==1
                    &&abs(All16LiDARCells[temp_i+m_i*m][temp_j+m_j*m].lowwer_zMax-All16LiDARCells[temp_i+m_i*m][temp_j+m_j*m].lowwer_zMin)<cell_height_threshold)
            {
                float x_dis=abs(All16LiDARCells[temp_i+m_i*m][temp_j+m_j*m].lowwer_xMax-All16LiDARCells[temp_i][temp_j].lowwer_xMax);
                float y_dis=abs(All16LiDARCells[temp_i+m_i*m][temp_j+m_j*m].lowwer_yMax-All16LiDARCells[temp_i][temp_j].lowwer_yMax);
                float dis=sqrt(x_dis*x_dis+y_dis*y_dis);
                if(dis<dis_limit)
                {
                    float cell_delta=dis*angle_limit_tan;
                    if(abs(All16LiDARCells[temp_i+m_i*m][temp_j+m_j*m].lowwer_zMax-All16LiDARCells[temp_i][temp_j].lowwer_zMax)<cell_delta
                     ||abs(All16LiDARCells[temp_i+m_i*m][temp_j+m_j*m].lowwer_zMax-All16LiDARCells[temp_i][temp_j].lowwer_zMin)<cell_delta)
                    {
                        i_ground_cell_vector.push_back(temp_i+m_i*m);
                        j_ground_cell_vector.push_back(temp_j+m_j*m);
                        All16LiDARCells[temp_i+m_i*m][temp_j+m_j*m].lowwer_ob=4;
                    }
                }
            }
            else if(All16LiDARCells[temp_i+m_i*m][temp_j+m_j*m].lowwer_ob==1)
                break;
            m++;
        }
        else
            break;
    }
}


void LidarPointCloudCorrect::InitPointcloud2MsgHeader(sensor_msgs::PointCloud2& cloud) {
  cloud.header.frame_id.assign("frame_id_");
  cloud.height = 1;
  cloud.width = 0;
  cloud.fields.resize(7);
  cloud.fields[0].offset = 0;
  cloud.fields[0].name = "x";
  cloud.fields[0].count = 1;
  cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
  cloud.fields[1].offset = 4;
  cloud.fields[1].name = "y";
  cloud.fields[1].count = 1;
  cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
  cloud.fields[2].offset = 8;
  cloud.fields[2].name = "z";
  cloud.fields[2].count = 1;
  cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
  cloud.fields[3].offset = 12; 
  cloud.fields[3].name = "intensity";
  cloud.fields[3].count = 1;
  cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
  cloud.fields[4].offset = 16; 
  cloud.fields[4].name = "tag";
  cloud.fields[4].count = 1;
  cloud.fields[4].datatype = sensor_msgs::PointField::UINT8;
  cloud.fields[5].offset = 17; 
  cloud.fields[5].name = "line";
  cloud.fields[5].count = 1;
  cloud.fields[5].datatype = sensor_msgs::PointField::UINT8;
  cloud.fields[6].offset = 18; 
  cloud.fields[6].name = "timestamp";
  cloud.fields[6].count = 1;
  cloud.fields[6].datatype = sensor_msgs::PointField::FLOAT64;
  cloud.point_step = sizeof(LivoxPointXyzrtlt);                                                                      
}

bool LidarPointCloudCorrect::pointCloudCalibration(SensorCalibrationParameterDef &param, pcl::PointCloud<pcl::PointXYZI> &pl_orig)
{
    int plsize = pl_orig.size();
    for (int i = 0; i < pl_orig.points.size(); i++)
    {
        Eigen::Vector3d pt_vec;
        pcl::PointXYZI point;
        point.x = pl_orig.points[i].x;
        point.z = pl_orig.points[i].z;
        pl_orig.points[i].z = point.z * cos(param.param.euler_angle_roll) - point.x * sin(param.param.euler_angle_roll);
        pl_orig.points[i].x = point.z * sin(param.param.euler_angle_roll) + point.x * cos(param.param.euler_angle_roll);

        point.y = pl_orig.points[i].y;
        point.z = pl_orig.points[i].z;
        pl_orig.points[i].y = point.y * cos(param.param.euler_angle_pitch) - point.z * sin(param.param.euler_angle_pitch);
        pl_orig.points[i].z = point.y * sin(param.param.euler_angle_pitch) + point.z * cos(param.param.euler_angle_pitch);

        point.x = pl_orig.points[i].x;
        point.y = pl_orig.points[i].y;
        pl_orig.points[i].x = point.x * cos(param.param.euler_angle_yaw) - point.y * sin(param.param.euler_angle_yaw) + param.param.pose_position_x;
        pl_orig.points[i].y = point.x * sin(param.param.euler_angle_yaw) + point.y * cos(param.param.euler_angle_yaw) + param.param.pose_position_y;
        pl_orig.points[i].z = pl_orig.points[i].z + param.param.pose_position_z;
    }
  return true;
}

pcl::PointCloud<pcl::PointXYZI> LidarPointCloudCorrect::pointCloudPreProcess(pcl::PointCloud<pcl::PointXYZI> &pl_orig)
{
    sensor_msgs::PointCloud2 pc;
    pcl::PointCloud<pcl::PointXYZI> pre_point_cloud;
    for (int i = 0; i < pl_orig.points.size(); i++)
    {
        if(((std::abs(pl_orig.points[i].x) > xLimit)||(std::abs(pl_orig.points[i].y) > yLimit))&&pl_orig.points[i].z < 3)
        {
            pre_point_cloud.points.push_back(pl_orig.points[i]);
        }
    }
    pcl::toROSMsg(pre_point_cloud,pc);
    dm_->aft_lidar_point_cloud_publisher_.publish(pc);
    return pre_point_cloud;
}
